
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       dev_mgr_check.c
  * @author     baiyang
  * @date       2021-11-7
  ******************************************************************************
  */

/*
  using checked registers allows a device check that a set of critical
  register values don't change at runtime. This is useful on key
  sensors (such as IMUs) which may experience brownouts or other
  issues in flight

  To use register checking call setup_checked_registers() once to
  allocate the space for the checked register values. The set the
  checked flag on any write_register() calls that you want protected.

  Periodically (say at 50Hz) you should then call
  check_next_register(). If that returns false then the sensor has had
  a corrupted register value. Marking the sensor as unhealthy is
  approriate. The bad value will be corrected
 */

/*----------------------------------include-----------------------------------*/
#include <stdio.h>
#include <string.h>
#include "dev_mgr.h"

#include <common/console/console.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  setup nregs checked registers
 */
bool devmgr_setup_checked_registers(gp_device_t dev, uint8_t nregs, uint8_t frequency)
{
    if (dev->_checked.regs != NULL) {
        rt_free(dev->_checked.regs);
        dev->_checked.n_allocated = 0;
        dev->_checked.n_set = 0;
        dev->_checked.next = 0;
    }
    dev->_checked.regs = (struct checkreg *)rt_malloc(sizeof(struct checkreg) * nregs);

    if (dev->_checked.regs == NULL) {
        return false;
    }

    dev->_checked.n_allocated = nregs;
    dev->_checked.frequency = frequency;
    dev->_checked.counter = 0;

    return true;
}

void devmgr_set_checked_register2(gp_device_t dev, uint8_t bank, uint8_t reg, uint8_t val)
{
    if (dev->_checked.regs == NULL) {
        return;
    }

    struct checkreg *regs = dev->_checked.regs;
    for (uint8_t i=0; i<dev->_checked.n_set; i++) {
        if (regs[i].regnum == reg && regs[i].bank == bank) {
            regs[i].value = val;
            return;
        }
    }

    if (dev->_checked.n_set == dev->_checked.n_allocated) {
        console_printf("Not enough checked registers for reg 0x%02x on device 0x%x\n",
               (unsigned)reg, (unsigned)dev->d.devid);
        return;
    }

    regs[dev->_checked.n_set].bank = bank;
    regs[dev->_checked.n_set].regnum = reg;
    regs[dev->_checked.n_set].value = val;
    dev->_checked.n_set++;
}

/*
  check one register value
 */
bool devmgr_check_next_register(gp_device_t dev)
{
    if (dev->_checked.n_set == 0) {
        return true;
    }
    if (++dev->_checked.counter < dev->_checked.frequency) {
        return true;
    }
    dev->_checked.counter = 0;

    struct checkreg *reg = &(dev->_checked.regs[dev->_checked.next]);
    uint8_t v, v2;

    if (dev->_bank_select) {
        if (!dev->_bank_select(reg->bank)) {
            // Cannot set bank
#if 0
            printf("Device 0x%x set bank 0x%02x\n",
                   (unsigned)get_bus_id(),
                   (unsigned)reg.bank);
#endif
            dev->_checked.last_reg_fail = *reg;
            return false;
        }
    }

    if ((!devmgr_read_registers(dev, reg->regnum, &v, 1) || v != reg->value) &&
        (!devmgr_read_registers(dev, reg->regnum, &v2, 1) || v2 != reg->value)) {
        // a register has changed value unexpectedly. Try changing it back
        // and re-check it next time
#if 0
        printf("Device 0x%x fixing 0x%02x 0x%02x -> 0x%02x\n",
               (unsigned)get_bus_id(),
               (unsigned)reg.regnum, (unsigned)v, (unsigned)reg.value);
#endif
        devmgr_write_register(dev, reg->regnum, reg->value, false);
        dev->_checked.last_reg_fail = *reg;
        dev->_checked.last_reg_fail.value = v;
        return false;
    }
    dev->_checked.next = (dev->_checked.next+1) % dev->_checked.n_set;
    return true;
}

/*
  check one register value, returning information on the failure
 */
bool devmgr_check_next_register2(gp_device_t dev, struct checkreg *fail)
{
    if (devmgr_check_next_register(dev)) {
        return true;
    }
    *fail = dev->_checked.last_reg_fail;
    return false;
}
/*------------------------------------test------------------------------------*/


